/*
* Copyright (c) 2010 Michael Collins
*
* This file is part of TerraFirma.
*
* TerraFirma is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* TerraFirma is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with TerraFirma.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <fixpoint.h>
#include <fixpoint_tables.h>

#define NEWTON_ITERATIONS 9

namespace fixedpoint
{
    /**Inverse Square Root function
    */
    fixpoint invsqrt(fixpoint a)
    {
        if (a <= fixpoint(fixpoint::RAW, 0))
            return a; //This is weird and stupid, but it works for all defined cases (namely 0)

        fixpoint x(fixpoint::RAW, invsqrt_lu[most_sig_1(a.raw())]); //use the lookup table to make a guess
        //Use the newton-raphson method to compute the reciprocal square root
        for (int i = 0; i < NEWTON_ITERATIONS; ++i)
        {
            //x = x/2 * (3 - a * x*x)
            x = (x >> (uint32_t)1) * (fixpoint(fixpoint::RAW, UNIT*3) - a*x*x);
        }

        return x;
    }

    /**Square Root function
    */
    fixpoint sqrt(fixpoint a)
    {
        return fixpoint(fixpoint::RAW, UNIT) / invsqrt(a);
    }

    /**Sine function (argument in units of radians)
    */
    fixpoint sin(fixpoint a)
    {
        int32_t normalized_argument = (a * F_2_PI).raw();
        uint8_t sample_point = (normalized_argument >> 8);
        uint8_t weight= normalized_argument & 0xff;
        int32_t sample0, sample1;

        if (normalized_argument & 0x00010000)
        {
            //Segment 1 and 3
            sample0 = (sample_point==0)? UNIT : (uint32_t) sin_lu[256 - sample_point];
            sample1 = (uint32_t) sin_lu[255 - sample_point];
        }
        else
        {
            //Segment 0 and 2
            sample0 = (uint32_t) sin_lu[sample_point];
            sample1 = (sample_point==255)? UNIT : (uint32_t) sin_lu[sample_point + 1];
        }

        int32_t result = interpolate(sample0, sample1, weight);
        if (normalized_argument & 0x00020000)
            //Negative region (region 2 & 3)
            return fixpoint(fixpoint::RAW, -result);
        else
            //Positive retion (region 0 & 1)
            return fixpoint(fixpoint::RAW, result);
    }

    /**Cosine function (argument in units of radians)
    */
    fixpoint cos(fixpoint a)
    {
        int32_t normalized_argument = (a * F_2_PI).raw();
        uint8_t sample_point = (normalized_argument >> 8);
        uint8_t weight= normalized_argument & 0xff;
        int32_t sample0, sample1;

        //Adjust the phase for a cosine lookup
        normalized_argument += UNIT;
        if (normalized_argument & 0x00010000)
        {
            //Segment 1 and 3
            sample0 = (sample_point==0)? UNIT : (uint32_t) sin_lu[256 - sample_point];
            sample1 = (uint32_t) sin_lu[255 - sample_point];
        }
        else
        {
            //Segment 0 and 2
            sample0 = (uint32_t) sin_lu[sample_point];
            sample1 = (sample_point==255)? UNIT : (uint32_t) sin_lu[sample_point + 1];
        }

        int32_t result = interpolate(sample0, sample1, weight);
        if (normalized_argument & 0x00020000)
            //Negative region (region 2 & 3)
            return fixpoint(fixpoint::RAW, -result);
        else
            //Positive retion (region 0 & 1)
            return fixpoint(fixpoint::RAW, result);
    }

    /**Tangent function (argument in units of radians)
    */
    fixpoint tan(fixpoint a)
    {
        //I don't really want to think about this too hard
        return sin(a) / cos(a);
    }

    /**Function that has the sole purpose of trying to optimize
     * finding both the sin and cosin of a single angle argument,
     * as is common for building rotation matrices
     */
    void sincos(fixpoint a, fixpoint& sin, fixpoint& cos)
    {
        int32_t normalized_argument = (a * F_2_PI).raw();
        uint8_t sample_point = (normalized_argument >> 8);
        uint8_t weight= normalized_argument & 0xff;
        int32_t inc_sample0, inc_sample1, dec_sample0, dec_sample1;

        inc_sample0 = (uint32_t) sin_lu[sample_point];
        inc_sample1 = (sample_point == 255)? UNIT : (uint32_t)sin_lu[sample_point+1];
        dec_sample0 = (sample_point == 0)? UNIT : (uint32_t) sin_lu[256 - sample_point];
        dec_sample1 = (uint32_t) sin_lu[255 - sample_point];


        int32_t inc_result = interpolate(inc_sample0, inc_sample1, weight);
        int32_t dec_result = interpolate(dec_sample0, dec_sample1, weight);

        switch ((normalized_argument >> 16) & 0x3)
        {
            //Segment sections
        case 0 :
            sin.raw_copy(inc_result);
            cos.raw_copy(dec_result);
            break;
        case 1 :
            sin.raw_copy(dec_result);
            cos.raw_copy(-inc_result);
            break;
        case 2 :
            sin.raw_copy(-inc_result);
            cos.raw_copy(-dec_result);
        case 3 :
            sin.raw_copy(-dec_result);
            cos.raw_copy(inc_result);
            break;
        default: //This should never occur
            break;
        }
    }

}
